/**
 * @file        motor_foc.c
 * @brief       电机FOC控制算法
 * @version     V1.2
 * @author      Jsir2022 (jsir2022@outlook.com)
 * @date        2024-02-26 22:06:29
 * LastEditors  Jsir2022
 * LastEditTime 2024-03-02 15:33:14
 * history		版本	日期		作者		说明
 * 				V1.0	2024-2-27	Jsir2022	正式发布
 *              V1.1	2024-2-29	Jsir2022	添加了clarke和park变换
 *              V1.2	2024-3-2	Jsir2022	添加了SVPWM。全部算法验证成功
 * Copyright (c) 2024 by Jsir2022, All Rights Reserved.
 * @verbatim
 * ==============================================================================
 *
 * ==============================================================================
 * @endverbatim
 */
#include "motor_foc.h"

/**
 * @brief   计算电角度 theta 对应的 sin 和 cos 值
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_calcSinCos(stFOC_Para_type *FOC_Para)
{
    FOC_Para->sin_val = sinf(FOC_Para->theta);
    FOC_Para->cos_val = cosf(FOC_Para->theta);
}

/**
 * @brief   Clarke变换将三相电流转换为 Alpha-Beta 坐标系下的电流
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_Clarke(stFOC_Para_type *FOC_Para)
{
    FOC_Para->i_alpha = FOC_Para->i_a;
    FOC_Para->i_beta = (FOC_Para->i_b - FOC_Para->i_c) * ONE_BY_SQRT3;
}

/**
 * @brief   逆Clarke变换将 Alpha-Beta 坐标系下的电压转换为三相电压
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_invClarke(stFOC_Para_type *FOC_Para)
{
    FOC_Para->v_a = FOC_Para->v_beta;
    FOC_Para->v_b = (-FOC_Para->v_alpha * 0.5f) + SQRT3_BY_2 * FOC_Para->v_beta;
    FOC_Para->v_c = (-FOC_Para->v_alpha * 0.5f) - SQRT3_BY_2 * FOC_Para->v_beta;
}

/**
 * @brief   Park变换将 Alpha-Beta 坐标系下的电流转换为 dq 坐标系下的电流
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_Park(stFOC_Para_type *FOC_Para)
{
    FOC_Para->i_d = FOC_Para->i_alpha * FOC_Para->cos_val + FOC_Para->i_beta * FOC_Para->sin_val;
    FOC_Para->i_q = -FOC_Para->i_alpha * FOC_Para->sin_val + FOC_Para->i_beta * FOC_Para->cos_val;
}

/**
 * @brief   逆Park变换将 dq 坐标系下的电压转换为 Alpha-Beta 坐标系下的电压
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_invPark(stFOC_Para_type *FOC_Para)
{
    FOC_Para->v_alpha = FOC_Para->v_d * FOC_Para->cos_val - FOC_Para->v_q * FOC_Para->sin_val;
    FOC_Para->v_beta = FOC_Para->v_d * FOC_Para->sin_val + FOC_Para->v_q * FOC_Para->cos_val;
}

/**
 * @brief   扇区法SVPWM
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_SVPWM(stFOC_Para_type *FOC_Para)
{
    // 周期
    float Ts = 1.0f;
    // 占空比
    float ta = 0.0f, tb = 0.0f, tc = 0.0f;
    // 系数K
    float k = (Ts * SQRT3) / FOC_Para->v_bus;
    // 由alpha-beta坐标系电压推出三相电压
    float va = FOC_Para->v_beta;
    float vb = SQRT3_BY_2 * FOC_Para->v_alpha - FOC_Para->v_beta * 0.5f;
    float vc = -SQRT3_BY_2 * FOC_Para->v_alpha - FOC_Para->v_beta * 0.5f;
    // 用N值判断所在扇区
    bool a = (va > 0.0f) ? 1 : 0;
    bool b = (vb > 0.0f) ? 1 : 0;
    bool c = (vc > 0.0f) ? 1 : 0;
    uint8_t sextant = (c << 2) | (b << 1) | a;

    switch (sextant)
    {
    case 3:
    {
        float t4 = k * vb;
        float t6 = k * va;
        float t0 = (Ts - t4 - t6) * 0.5f;

        ta = t4 + t6 + t0;
        tb = t6 + t0;
        tc = t0;
        break;
    }
    case 1:
    {
        float t6 = -k * vc;
        float t2 = -k * vb;
        float t0 = (Ts - t2 - t6) * 0.5f;

        ta = t6 + t0;
        tb = t2 + t6 + t0;
        tc = t0;
    }
    break;

    case 5:
    {
        float t2 = k * va;
        float t3 = k * vc;
        float t0 = (Ts - t2 - t3) * 0.5f;

        ta = t0;
        tb = t2 + t3 + t0;
        tc = t3 + t0;
    }
    break;

    case 4:
    {
        float t1 = -k * va;
        float t3 = -k * vb;
        float t0 = (Ts - t1 - t3) * 0.5f;

        ta = t0;
        tb = t3 + t0;
        tc = t1 + t3 + t0;
    }
    break;

    case 6:
    {
        float t1 = k * vc;
        float t5 = k * vb;
        float t0 = (Ts - t1 - t5) * 0.5f;

        ta = t5 + t0;
        tb = t0;
        tc = t1 + t5 + t0;
    }
    break;

    case 2:
    {
        float t4 = -k * vc;
        float t5 = -k * va;
        float t0 = (Ts - t4 - t5) * 0.5f;

        ta = t4 + t5 + t0;
        tb = t0;
        tc = t5 + t0;
    }
    break;
    }
    FOC_Para->T_deltaA = 1.0f - ta;
    FOC_Para->T_deltaB = 1.0f - tb;
    FOC_Para->T_deltaC = 1.0f - tc;
}

/**
 * @brief   FOC算法的主要函数，用于计算电机的电流和电压控制
 * @note
 * @param   {stFOC_Para_type} *FOC_Para FOC参数结构体指针
 * @return  {*}
 */
void Motor_FOC_calcAll(stFOC_Para_type *FOC_Para)
{
    Motor_FOC_calcSinCos(FOC_Para);
    Motor_FOC_Clarke(FOC_Para);
    Motor_FOC_Park(FOC_Para);
    Motor_FOC_invPark(FOC_Para);
    Motor_FOC_SVPWM(FOC_Para);
}
